/*
 * kalman_filter.h
 *
 *  Created on: 2024-03-12
 *      Author: lidq
 */
#ifndef __SRC_KALMAN_FILTER_H
#define __SRC_KALMAN_FILTER_H

#include <typedef.h>
//
#include <matrix.hpp>

typedef struct
{
	// 控制周期
	double dt;
	// 过程噪声协方差
	double S_val;
	// 观测噪声协方差
	double R_val;
	// 最优估计：位置
	double est_pos;
	// 最优估计：速度
	double est_vel;
	// 观测向量
	s_Matrix measurement;
	// 定义状态向量 [位置, 速度]
	s_Matrix state;
	// F：状态转移矩阵
	s_Matrix F;
	// B：输入控制矩阵
	s_Matrix B;
	// 定义误差协方差矩阵
	s_Matrix P;
	// Q：过程噪声协方差矩阵
	s_Matrix Q;
	// 观测矩阵
	s_Matrix H;
	// 观测噪声协方差
	s_Matrix R;
	// 卡尔曼增益
	s_Matrix K;

	s_Matrix temp1;
	s_Matrix temp3;
	s_Matrix temp2;
	s_Matrix temp4;
	s_Matrix temp5;
	s_Matrix temp6;
	s_Matrix temp7;
	s_Matrix temp8;
	s_Matrix temp9;
	s_Matrix temp10;
	s_Matrix temp11;
	s_Matrix temp12;
	s_Matrix temp13;
	s_Matrix temp14;
	s_Matrix temp15;
	s_Matrix temp16;
	s_Matrix temp17;
	s_Matrix temp18;
	s_Matrix temp19;
	s_Matrix temp20;
} kalman_s;

// 初始化
void kalman_init(kalman_s* kalman, double dt, double S_val, double R_val);

// 销毁
void kalman_destroy(kalman_s* kalman);

// 过程更新
void kalman_update(kalman_s* kalman, double p_accel, double m_pos, double m_vel);

// 计算数据方差
double kalman_variance(double* data, int size);

#endif